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SECOND GENERATION SIX-DEGREE-OF-FREEDOM 
VIRTUAL PIVOT HAND CONTROLLER 

BACKGROUND OF THE INVENTION 

L Field of the Invention 

5 The present invention relates to hand controllers and more particularly to hand 

operated devices for operating remote systems such as flight control systems in aircraft or 

^acecraft or for controlling robotics or land vehicle mechanism and to provide desired power 

assistance and "feel" to the control. 

2. Description of the Prior Art 

10 Hand controllers are well known in the art as, for example, the virtual pivot hand 

controller of U.S. Patent 4,962,448 to Joseph DeMaio et al., issued October 9, 1990, and 

assigned to the assignor of the present invention. Six-degree-of-freedom mechanisms are also 

known in the art as, for example, the six-degree-of-freedom hand controller of Patent 

4,914,976 issued April 10, 1990 to Charles E, Wyllie and the six-degree virtual pivot 

15 controller of patent application Serial No. 07/636,318 filed December 31, 1990 in the name 

of Jon M. Blomberg et al. assigned to the assignee of the present invention. Another device 

known as a Stewart platform has been modified to consist of a hand grip mounted on a 

platform which is connected at spaced locations to one end of three articulated legs while the 

other end of each leg is connected to be "actuated." The difficulty encountered in the 

20 modified Stewart platform is that if one attempted to use it for a realistic type hand controller 

such as shown in the Wyllie Patent, the forces felt by the operator as he moves the hand grip 

in various directions would not be realistic. For example, if the other end of the articulated 

legs were mounted for actuation with a pair of scissor springs to increase the force feedback 

as the legs move, which is common in the art as described in the copending application of 

25 Israel Menahem, Serial No. 07/655,740 filed Febmary 14, 1992 or the mechanism shown in 

the copending application of Israel Menahem et al.. Serial No, 07/738,255 filed July 30, 1991, 

both assigned to the assigner of the present invention most motion would be opposed by a 

complicated combination of spring forces. In fact, regardless of the actuator, motion of the 

hand grip would, in most cases, bring into play various combinations of opposing forces which 

30 would make obtaining the proper "feel" of the hand grip substantially impossible. 

Furthermore, actuating of the other ends of the articulated legs by some sort of motive means 

introduces complex dynamic and kinematic equations of motion that make it extremely 

difficult to generate motions of the hand grip that would reflect the desires of the operator. 
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STIMMARY OF THE INVENTION 
The present invention operates with a mechanical arrangement similar to the modified Stewart 
platform but connects each of the three articulated legs to a special motor/pickoff combination 
such as has been described in the copending patent applications of Robert E. DeMers, Serial 

5 No. 07/824,632 and 07/824,633 both filed January 23, 1992 and assigned to tiie Assignee of 
the present invention. These motor/pickoffs operate to provide an output arm with the 
c^ability of rotating around two mutually orthogonal axes. The three motor/pickoffs are such 
that no motor is required to move the mass of another motor. This accordingly operates to 
minimize the inertia of the system. The hand grip is connected to a six-axis force/torque 

10 sensing device which produces signals indicative of forces and torques being applied in any 
of the six directions at the sensor location. The processor modifies them to transform them 
into values which would have been sensed at a "virtual pivot" (e.g., wrist joint) or at any fixed 
point relative to the base plate other than the sensor mounting location. These signals are a 
processor which operates to determine which of the motors should be activated and how much 

15 resistive force should be appUed in order to move the hand grip with the proper "feel" in the 
direction indicated by the sensor. Motor control systems in the processor drive the motors in 
the proper directions and feedback ftom various sensors in the system provides information 
to the processor which tells it how the position of the hand grip contmue to differ from the 
desired position. The reverse force which is applied may be customized to an individual's 

20 desires so that each operator will have the proper "feel" according to his particular liking. 
Accordingly the operator may have a customized hand grip, without altering the mechanical 
components, to able him to move the hand controller as he chooses with a "feel" that is best 
for his specific tastes. 

TtPTPF nRSCRTPTION OF THE DRAWINGS 
25 Figure 1 shows a perspective and partly schematic view of the six-degree-of- 

freedom hand controller of the present invention. 

Figure 2 shows a graph of the displacement of the hand controller and the 
corresponding resistive force or torque necessary to provide the realistic "feel." 

Figure 3 shows a block diagram of the processor and its internal and external 
30 components. 

Figure 4a, b and c shown the angles, and forces used in analyzing the kinematics 
and dynamics of the present invention. 

Figure 5 shows the geometry of one of the articulated legs of the present 
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invention. 

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS 
In Figure 1 a hand grip 10 is shown mounted on a force/torque sensor 12 which 
is mounted on a more or less triangular plate 14. Force/torque sensor 12 may be of the type 
5 sold by Assurance Technologies, Inc. as the ATI F/T Sensor and operates to produce a 
plurality of output signals on an output identified by reference numeral 16 whenever any 
forces and torques are applied to the hand grip in any of the six directions shown by arrows 
20» 21, 22, 23, 24 and 25. Arrow 20 represents yaw motion about an axis shown as dash-dot 
line 27, arrow 21 represents roll motion about an axis shown as dash-dot line 28, arrow 22 
10 represents pitch motion about an axis shown as dash-dot line 29, arrow 23 represents 
forward/backward motion along axis 28, arrow 24 represents right/left motion along axis 29, 
and arrow 25 represents up/down motion along axis 27. The signals indicative of the forces 
and torques applied by the operator in one or more of these directions are presented by output 
16 to a processor 30 which determines from them which directions the operator desires the 
15 hand grip 10 to move. 

Triangular platform 14 forms one part of a Stewart platform the rest of which 
comprises three, articulated legs having upper leg portions 32, 33 and 34, respectively, 
connected by rod end bearing connections 35, 36 and 37, respectively, to the comers of the 
triangular plate 14. Upper leg portions 32, 33 and 34 are pivoted at connections 40, 41 and 
20 42 respectively to lower leg portions 45, 46 and 47 which extend downwardly at a angle from 
upper leg portions 32, 33 and 34 respectively and are connected to a base plate 49. 

We have discovered that if the lower leg portions 45, 46 and 47 are connected to 
the base plate 49 by couplings such as are described in the above referred to DeMers 
copending applications, so that each leg is able to move in two mutually orthogonal directions, 
25 then it is possible to obtain all six directions of motion as shown by arrows 20, 21, 22, 23, 
24 and 25 as deshed. Furthermore, by controlling the speed of operation of the motors, the 
desired "feel" can be generated. 

Lower leg portion 45 is shown connected to a first coupling 51 which is in turn 
coimected to two motor/pickoffs 53 and 54 each of which is capable of sensing the position 
30 of and driving the lower leg portion 45 about one of two mutually orthogonal axes. Within 
each of the motor/pickoffs 53 and 54 there is contained a position sensing device capable of 
producing an output signal indicative of the angular rotation of lower leg portion 45 about one 
of the two mutually orthogonal axes. 
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Motor/pickoff 53 has an output shown by a line with reference numeral 57 which 
leads to the processor 30 to provide positional information and processor 30 is shown having 
an output 59 which leads to the input of motor/pickoff 53 so as to cause the motor to drive 
in a prescribed fashion as directed by the processor 30. Processor 30 also has a program input 

5 60 which is used to characterize the motor driving in accordance with the particular operator's 
"feel" requirements. Similarly the position sensing device within motor/pickoff 54 operates 
to produce an output signal indicative of the rotation of lower arm portion 45 about the other 
of the two mutually orthogonal axes and this signal is presented on an output shown by 
reference numeral 61 which leads to the processor 30. Processor 30 has an output shown by 

10 a line with reference numeral 63 connected to the motor/pickoff 54 so as to cause the motor 
in a prescribed fashion and in accordance with the program 60. 

Similarly, lower leg portion 46 is connected to a coupling 62 which is connected 
to two motor/pickoffs 63 and 64 each of which is capable of sensing the position of and 
driving the lower leg portion 46 about one of two mutually orthogonal axes. Within each of 

15 the motor/pickoffs 63 and 64 there is contained a position sensing device capable of producing 
an output signal indicative of the angular rotation of the lower leg portion 46 about one of the 
two mutually orthogonal axes. Motor/pickoff 63 has an output shown by a line with reference 
numeral 65 connected to processor 30. Processor 30 has an output shown by a line with 
reference numeral 67 which is connected to the motor portion of motor/pickoff 63 and 

20 operates to drive shaft 46 by the desired amount along the first of the mutually orthogonal 
directions as directed by program input 60. Motor/pickoff 64 has an output shown by a Une 
with reference numeral 73 which leads to the processor 30 to provide a signal indicative of 
the motion of arm 46 about the other of the two mutually orthogonal axes about which arm 

46 moves. Microprocessor 30 has an output shown, by a line with reference numeral 75 
25 connected to the motor/pickoff 64 to drive lower leg portion 46 about the other of the two 

mutually orthogonal axes as directed by program input 60. 

Finally, lower leg portion 47 is connected in similar fashion to a third coupling 
81 which is connected to two motor/pickoffs 83 and 84 to cause motion of arm 47 about two 
mutually orthogonal axes. Motor/pickoff 83 has an output shown by a line with reference 
30 numeral 85 which leads to the processor 30. Processor 30 is shown producing an output along 
an output shown by reference numeral 87 to motor/pickoff 83 and operable to drive the arm 

47 about a first of the two mutually orthogonal axes as directed by program input 60.' 
Motor/pickoff 84 has as an output shown by a line with reference numeral 93 which leads to 



wo 95/04959 



-5- 



PCTAJS93/07469 



the processor 30 and is indicative of the motion of arm 47 about the other of the two mutually 
orthogonal axes about which arm 47 moves. Processor 30 is also shown producing an output 
signal on a line shown by reference numeral 95 to motor/pickoff 84 operable to drive lower 
arm 47 about the other of the two mutually orthogonal axes as durected by program input 60. 
5 In addition to the pickoffs contained in the motor/pickoff combinations above 

described, each of the three articulated arms has an additional pickoff 97, 98 and 99, 
respectively, connected at the pivot joints between the upper leg portions 32, 33 and 34 and 
the lower leg portions 45, 46 and 47 and these pickoffs produce outputs along lines shown by 
references numerals 100, 101 and 102, respectively, to the processor 30 in order to give 
10 processor 30 additional information about the movement of the hand grip 10 and platform 14 
with respect to the base plate 49. While the pickoffs contained in the motor/pickoff 
combinations 53 and 54, 63 and 64 and 83 and 84 described above would be sufficient to 
provide the processor 30 with sufficient information to enable it to determine the motions of 
the hand grip 10 and platform 14 with respect to base plate 49, the program for solving the 
15 kinematic and dynamic equations of motion of the system is somewhat simplified by having 
the additional inputs provided by the pickoff 97, 98 and 99, respectively. In addition to the 
signals to processor 30 which have been thus far described, each motor 53, 54, 63, 64, 83 and 
84 includes a tachometer which produces a signal indicative of the motor's speed and these 
signals are also presented to the processor but,for simplicity, have not been shown in Figure 
20 1 but will be described in connection with Figure 3. 

The processor 30 is programmed at input 60 to determine the desired motion of 
hand grip 10 in each of the six desired axes to drive the motors in the motor/pickoff 
combinations accordingly. The motion of hand grip 10 is not, however, frictionless since it 
is desired to provide the hand grip with a "feel" which is produced by forces presented to the 
25 three articulated arms from the motor/pickoff combinations above described. In other words, 
as the hand grip moves from a known position it is assisted in this motion, to a certain extent, 
by the motor/pickoff combinations but as the hand grip gets further and further froni the 
original point the amount of force necessary to produce additional movement of the hand grip 
increases. In general the motion from null is normally accompanied by resistive "feel" forces 
30 applied by the motors in accordance with the curve shown in Figure 2. In Figure 2 the 
horizontal axis shows the motion of the hand grip 10 and platform 14 in any one of the 
directions from an assumed null position directly above the center of base plate 49 while the' 
vertical axis shows the amount of resistive force necessary to provide the "feel" desired. It 



wo 95/04959 



-6- 



FCTAJS93/07469 



wiU be noticed that from the nuU position there is a small amount of force which can be 
applied before there is any motion but when the motion starts, the resistive force is a relatively 
constantly increasing force along the Une shown by reference numeral 110. When it reaches 
a position A, the resistive force increases slighdy to position B with no significant increase 
5 in motion. This is used to provide a "detent" feel for the hand grip 10. In other words from 
null to point A there is a continual increase in the force felt by the operator but as soon as it 
reaches point A it jogs into a "detent" requiring additional force to move it again. This would 
be the natural feel for the equipment at that position. Beyond point B the operator can force 
the hand grip 10 to move further along a relatively constantly increasing resistive force line 
10 shown by reference num^al 113 to a point C. At point C no further motion is permitted and 
this corresponds to the absolute limit of motion of the hand grip 10. By properly programming 
the processor 30, the curve such as shown in Figure 2 may be modified for different ones of 
the directions and for different operators desires. Such modifications would usually produce 
different slopes to the lines 110 and 113 but would normally be m a generalized shape such 
15 as shown in Figure 2. It is, of course, possible to modify these curves to conform to any 
desired resistive force. For example, the detent could be eliminated or the straight Ime 
portions 1 10 and 1 13 could be curves if desired. 

Figure 3 shows the block diagram of the system wherein the modified Stewart 
platform of Figure 1 is shown as a box 120, and the portion sunounded by dashed line 130 
20 represents the processor 30 of Figure 1. The output 16 from the ATI sensor 12 of Figure 1 
is shown on a line 132 leading to an ATI box 134. ATI box 134 is part of the force/torque 
sensor 12 of Figure 1 and may be included therein but is shown in Figure 3 as a separate box 
for producing the electrical signals representative of the forces and torques being applied by 
the operator in the six directions 20-25 of Figure 1. The outputs from the various pickoffs 
25 of Figure 1 along lines 57, 61, 65, 73, 85. 93, 101, 102 and 103 are represented in Figure 3 
by a single line 136 leading to a box 138 labelled "Reverse Kinematics." The "Reverse 
Kinematics" box 138 operates on the signals from line 136 to mathematically determine the 
present position of the hand grip 10 and the triangular platfonn 14 with respect to the base 
plate 49 of Figure 1. These mathematics will be described heremafter. The tachometer 
30 signals from the various motors in Figure 1 are shown on a single line 140 in Hgurc 3 leading 
to a box 142 labelled "Motor Control" which contains circuitry for controlling the start, stop 
and speed of the motors m accordance with the commands from the processor and the 
tachometer feedback signals on line 140. The motor control signals on lines 59, 63, 65, 75, 
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87 and 95 of Figure 1 are represented in Figure 3 by a single line 144 coming from the 
"Motor Control" box 142. Finally, the program input 60 of Figure 1 is shown, by way of 
example as coming from a pair of keyboards 146 and 148 connected respectively to an "Input 
& Command Process" box 150 and the ATI box 134 by lines shown with reference numerals 
5 152 and 154. 

As mentioned, the ATI box 134 produces signals representative of the forces and 
torques commanded by the operator through the hand control and these signals are transferred 
to the "Input & Conamand Process" box 150 by way of lines 156 (for linear forces) and 158 
(for torques). Also, as mentioned, the "Reverse Kinematics" box 138 determines 

10 mathematically the current position of the upper platform 14 with respect to the lower 
platform 49 and this information is transferred to the "Input & Conunand Process" box 150 
via a line 154. "Input & Command Process" box 150 operates on the information concerning 
the present position of the upper platform 14 with respect to the lower platform 49, the 
commanded forces and torques as determined by the ATI box 134 and any modifications to 

15 the resistive force produced by the keyboards 146 and 148 to determine the desired position 
of the upper platform 14 with respect to the lower platform 49, and sends this information to 
a "Forward Kinematics" box 160 via and output shown as line 162. "Forward Kinematics" 
box 160 operates on the input form Une 162 to mathematically determine which motors should 
be actuated, in which direction and with what resistive force to cause upper platform 14 to get 

20 to the desired position with respect to lower platform 49. The mathematics involved will be 
explained hereinafter. The information from the "Forward Kinematics" box is presented to 
the "Motor Control" box 142 via an output shown as line 166, and as indicated above, "Motor 
Control" box operates the proper motors in the modified Stewart platform box 120 to produce 
the desire result. The various operations are accomplished in very small steps and very 

25 rapidly. The outputs from the sensors are processed and the motors are controlled about 100 
time each second so that the operator does not detect the steps by feel. Continuously updating 
the data and re commanding the motors this rapidly allows the force stick to move to the 
desire position smoothly with no detectable overshoot. 

The mathematical analysis performed by the "Reverse Kinematics" box 1 38 

30 will now be described with reference to Figures 4a, 4b, 4c and 5. 

With regard to the coordinate systems, seven separate cartesian coordinate jframes 
are necessary in order to describe the kinematics and dynamics of the basic J-VPHC concept. 
Three systems are associated with the legs, one defines the base plate and one defines the 
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platfom frame. Two additional frames to define the "virtual pivot" coordinate frames will 
be introduced later. These reference frames, shown in Figure 4, are described as follows: 

Base Frame (Sg) - This cartesian frame (with origin at the base centroid) has the 
Xb axis directed toward base pivot point Bl and Zb directed vertically downwards. Constant 
5 Vectors Rb„ Rb2 an^ Rb3 define the leg pivot points with respect to the origin of Sg as shown 
in Figure 4(c). 

Lower Leg Frames (S„ and Sj) - These frames have their origins at points Rb„ 
Rb2 and Rb3 (See Figure 5). One joint rotates the lower leg through a roll angle <|) (i.e., <})„ ^2 
or <|)3) whose axis or rotation is radial from the origin of S^. Roll angles are zero when the 
10 leg is in the vertical plane through tiie axis of rotation as shown in Figure 4(b). Each lower 
leg is tiien rotated through a pitch angle 0, as shown in Figure 4(a). Zero pitch angle occurs 
when lower and upper legs form a right angle. The y^i (i=l, 2^ 3) axes are defined as 
perpendicular to the plane formed by and position vectors Rg,, Rb2 and Rbj, respectively. 

Platform Frame (Sp) - This frame has its origin at die centroid of the plane defined 
15 by the three centers of each joint. The axis is directed toward the ball joint at PI whose 
position vector is Tpj, the Z, axis is normal (downwards) to the plane. The Yp axis is paraUel 
to die vector [Fj-rj]. Vectors 7p„ Tpz and 7^3 (referred to Sp) are fixed in Sp of length a,. 
Ball joint locations are R, with respect to Sg. 

With regard to the Reverse Kinematics, given the nine angles O^, ©uil. where 
20 i= 1 ,2,3 and the constant parameters Lj and are lower and upper link lengths, Ri, and rp are 
die radius of the base and platform pivots and 7,. Y2 and Y3 are the mounting angles of the base 
pivots (B„ B2, B3) then tiie position vectors R; in Sb of each ball joint (illustrated in Figure 
3) are then given by: 
Equation 1 

R. = RBi^Dr,i = 1^3 

25 where D; is the vector between Rb; and its corresponding ball joint expressed in Sb 
coordinates. The position vector and attitude (i.e., Euler angles) of the platform relative to 
the base coordinate frame Sb are tiien obtained from D; and Rj^ vectors in a direct manner. 
The origin of the platform coordinate frame Sp is at the centroid of the plane defined by three 
ball joints, the Xp axis is toward P, and the Yp axis parallel to the line joining Pj to P3. 

30 The position vector Rp from the base plate origin to platform origin is then given 

by: 
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Equation 2 



since 



i 

Here (i = 1,2,3) are the vectors from each leg mounting location on the base 
(B„ Bj, B3) to their respective ball joints (P„ Pj, P3) as shown in Figure 3(a). 
5 Coordinate frames S,, 83 and S3 are obtained by first rotating % about the Zb axis 

by O"", -120*", and 120°, respectively, flien a roll rotation fy^ about the resulting Xgj axes (Shown 
in Figure 3(c). The geometry for all three legs is identical and is shown in Figure 3. 

With respect to the reverse kinematics computations, in each coordinate system 
(S„ 82, S3) the location of the ball joint relative to the base pivot point can be expressed as 
10 Equation 3 



0 

(i^sine^. + L^cosej^)]^^ 



where and ©uj are the two joint angles of the i* leg. The three vectors, Dj, when 
transformed into Sb coordinates are given by 
Equation 4 



where 
15 Equation 5 

In the above equations C() and S() represent sine and cosine fiinctions. Rotation 
matrices E(<})), E„ E2 and E3 represent rotations from Sg to Sj. As a result, matrix transpose 
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is used to form the inverse transformation (denoted by superscript T). Note that the subscript 
i is dropped for the £(<});) expression (for simplification of these expressions). 

the process is then determined by first computing the following vectors: 



Equations 6. 7. 8 

10 The overall process to find tiie position of vector Rp of the platform origin then 

consists of the following: 
Stet> 1 -Compute Ball Joint Position 

From the nine measured shaft angles <|)i, 6^, and (i = 1.2,3). Compute the 
vectors D; from leg base to Pj as follows: 
15 First, compute the vectors shown above as Equations 6, 7, 8. 

Second, compute Rp, Rj. Rj, Rj vectors to the platform origin Sp and ball joints 

by 

Equation 9 
where 
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^1 = 



-CeiL^SQ^+I^CQ^jj] 



^3 = 



-1^0 
2 2 



2 2 
0 0 0 

2 2 

Vl -1 0 
2 2 



1 


0 


0 




0 




-5*2 




0 


5<j>2 







0 



1 0 0 
O C4>3 5*3 
0 -5*3 C4)3 



0 



Equation 10 









-la 
2 


B 




1 

2 * 




0 

0 . 




-[fl 
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5 Step 2" Euler Rotation Matrix 
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The radius from platfonn center to ball joints is ap. Then unit vectors in Sp are 



given by 
Equation 11 



The three Xg, Yb, Zb components (of each unit vector t^, Yp, 2^) are the 
5 direction cosines and form the rows of the Euler rotation matrix (from Sb to Sp). That 
is: 



Eauation 12 



(x,'^(K■yB)(^p^^ 
^p'^^p'yB)^p'^ 



Step 3 - Compute Euler Angles 

All attitude information needed to define the orientation between Sb and Sp frames 

10 is given by Ep^ and its transpose. Many differing sets of Euler angles are used in the 
literature, and no one set is better as (if correctly applied in sequence) they each result in the 
same finite rotation. One set is commonly used in the aerospace industry and involves 
successive yaw rotation (V) about the Zb axis, then pitch (6) about the new Y axis, and finally 
roll (<» about the final % axis. In matrix form, this Euler matrbt is given by: 

15 Equation 13 
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1 0 0 

0 C(t> 5<|> 

0 -s4> c4> 



ce 0 -58 

0 1 0 

se 0 ce 



ci|r sx|f 0 
-si|f ci|r 0 
0 0 1 



or 

Equation 14 



cec^ CeSij; -56 

[C4xsecT|; +s<j)ST|r] [c<j)s9si|r -s^S'^]c^c% 



From inaction. 
Equation 15 

RoU*^ = tan"^ [fijs/Csa] 
Pitch = sin'*[-Ci3] 
YawTlr^ = tan-i [fiia/eii] 

5 Hence, the above computations can be applied to compute roU-pitch-yaw angles 

from the equations computed previously. Note that with the Euler rotation matrix between any 
two frames, the above Euler angle equations refer to the base frame. This will be of use later 
when offset frames from Sp or Sb are introduced for the virtual pivot concept. 

The forward kinematics transformation assumes that the platform position vector 

10 Rp and the Euler angles are known for some future time instant. A new problem is then to 
compute the six shaft angles 6^, <l>i (i = 1.2,3) necessary to achieve the desired Rp, %, 
values. 

This task is solved in two steps. First, the Euler matrix (corresponding to 
new values R*p, e'p, Xj/'p, is- computed. This process is independent of the desired 
15 translation. Second, the mechanism is assumed to produce the desired translation and attitude 
change. The position of each mid-joint is computed as the intersection of two circles (in the 
plane of each leg) with the radius L, and centered on base pivots and ball joints, 
respectively. 
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Step 1 - Compute Future Euler Matrix 

This matrix is computed from the previously given equation (14)(and denoted as 
E*pm) from the desired values R*p, Y*p obtained from the control law process. 

Step 2 - Compute Roll Shaft Angles 

Assume that the new platform position vector is defined by Figure 4 and force 

inputs, i-e. 
Equation 16 



P is 



The location of the balls joints (in Sp) is obtained from 



10 



Equation 17 



'■pi = 



p 
0 

0 



1 



3 

lN2j 



P3 



1 

0„ 

2 " 



inS. 



■where ap is the radial distance from the platform frame's origin. 
Then, 

Equation 18 







ft- 




ft 





15 Rotating vectors D'j into respective Sj is accomplished by the following process which yields 
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Equation 19 

since X and Z components lie in the plane of each joined leg. Here, D'j = 
[Xf*yf*zff . 



Step 3 - Compute Joint Position 



Computing cos (t)^ = [l - (^il^ifY ^^^es the computation of E (^^ 
10 which enables 
Equation 20 



Si 



The equations for the two circles are 



Equation 21 



These equations have two solutions which define the two intersections shown in Figure 4. 
15 The desired solution for the joint coordmates X'j and ZTj is: 
Equation 22 
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1 + 



+ b 



b 




1 + 




2 


^^ 




[ ^ JJ 



a 

2R 



1 + 

I I R' 



where 



If links Li,L2 for each leg are all of equal length, the above equations reduce to: 



Equation 23 



(iLil 



2R^ ^ 



5 Step 4 - Compute Leg Shaft Angles 

Having completed the previous st^, the lower shaft angle Q\i for each leg is 

given by: 
Equation 24 

= tan-^ (-X}jZ^);i = 1^3 
where (Xf* .Z/) are the solutions to the three sets of quadratic equations (wi± the largest 

10 positive X-root). The passive leg segments 1^ (which are driven by L,) have angles 
Equation 25 

el, = sin-»[(x;aJ/L2];i = 1,2,3 



Note that values outside of 
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Equation 26 

denote a serious problem as the jointed leg segment wUl pass through a toggle configuration 
if this occurs. At the completion of this computational step, the six shaft angles <^*Li, (i - 
1,2,3) are output to the shaft-angle motor drive controller. 
5 Virtual Pivot Modification 

The patented virtual pivot concept described in the above mentioned U.S. Patent 
4,962,448, when applied to the kinematics described earlier, requires use of two additional 

coordinate frames, i.e.: 

i. Relative-to-Platform System (SJ - defined by a fixed offset vector Typ 
10 from Sp to S^ and an Euler rotation matrix Eyp. This frame translates with 

Sp and may be defined at the center of an operator's wrist joint; and 
ii. Relative to Base System (Svb) - defined by an offset position vector Ryb which 
is fixed and does not translate with Sp. 

It is assumed that a logic discrete selects the appropriate virtual pivot system (or 
15 none at all). The default is the absence of any. The utility of Svb is in its ability to simulate 
floor or ceiling mounted operator-sensed pivot points. The unique ability of Syp is to move 
the sensed pivot point to the center of the operator's wrist (for example) and thereby decouple 
translation and gap attitude commands. 

Modifications to the kinematics are as follows: 
20 1. The measured position vector of the platforai is changed to the measured 

position at the virtual pivot origin, i.e. 



25 Equation 27 



or 

Eouation 28 

2. Euler angles are computed from 
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Equation 29 

It is therefore seen that I have provided a six-degree-of-freedom hand controller 
which operates with motorized resistance and applied proper resistive force so as to provide 
a correct "feel" and in which no motor is require to drive the weight of another motor. 
5 Although the present invention has been described with reference to preferred embodiments, 
workers skilled in the art will recognize that changes may be made in form and detail without 
departing from the spirit and scope of the invention. 



wo 95/04959 



-19- 



FCTAJS93/07469 



WHAT IS CLAIMED IS: 

1, A six-degree-of-fireedom controller comprising: 

first and second platforms (14,49) spaced apart by first, second and third 
articulated legs (32-45,33-46.34-47) each having a first end (35; 36,37) 
5 pivotally attached to the first platform, a second end (51; 62,81) pivotally 

connected to the second platform and moveable about first and second 
mutually perpendicular axes; 
hand controller means (10) mounted on the first platform and operable to respond 
to a force applied by a controller's hand in a desired direction to produce 
10 first signals indicative thereof; 

first and second motive means (53-54,63-64,83-84) connected to the second end 
of each leg and operable in response to input control signals to move the 
second ends in directions which will allow motion of the hand controller 
in the desired direction and provide enough resistance to the force applied 
15 by the controllers hand to give flie proper "feel" to the operation of the 

hand controller; 

sensing means (53-54,63-64,83-84) attached to the second end of each leg and 
operable to sense motion about first and second mutually perpendicular 
axes; and, 

20 processor means (30) connected to receive the signals from the sensing means and 

from the hand controller and to modify them by a characterized amount 
predetermined according to the operator's desires and to supply the 
modified signals to the first and second motive means as the input control 
signals. 

25 

2. Apparatus according to claim 1 further including means (60) associated with the 

processor means to provide the information necessary for the processor to perform the 
characterization of the signals in accordance with the operator's desires. 

30 3- A4)paratus according to claim 2 wherein the further means is programmed for the 

desires of each individual operator. 



4. 



Apparams according to claim 1 further including pivot means (40,41,42) between 
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the first and second ends of each articulated leg and pivot sensing means (97.98,99) to supply 
additional signals indicative of the amount of pivot of each pivot means to the processor 
means, the processor means using the additional signals with the signals from the hand 
controller and the sensing means to produce the input control signals. 

5 

5. Apparatus according to claim 4 further including means (60) associated with the 

processor means to provide the information necessary for the processor to perform the 
characterization of the signals in accordance with the operator's desires. 

10 6. Apparatos according to claim 5 further including pivot means (40,4 1 ,42) between 

the first and second ends of each articulated leg and pivot sensing means (97,98,99) to supply 
additional signals indicative of the amount of pivot of each pivot means to the processor 
means, the processor means using the additional signals with the signals from the hand 
controller and the sensing means to produce the input control signals. 

15 

7. Apparatos according to claim 1 wherein the signals from the hand controller are 

indicative of linear forces along three mutually perpendicular axes and indicative of rotational 
torques about three mutually perpendicular axes. 

20 8. Apparatos according to claim 7 wherein the processor means includes a reverse 

kinematics circuit (138) to receive the signals firom the sensing means and produce an output 
in accordance the present position of the second ends of the articulated legs about the two 

axes associated therewith; 

a force responsive circuit (134) to receive the signals from the hand controller and 
25 to produce an output in accordance with the linear forces and rotational 

torques fi-om the hand controller; 
processing circuitry (150) operable in accordance with the outputs from the 
reverse kinematics circuit and the force responsive circuit to characterize 
the signals and produce an oixtpm (162) in accordance with the operator's 
30 desires; forward kinematics circuit (160) to receive the ouQ)ut from the 

processing circuitry and produce signals (166) indicative of the desired 
motion of the second ends of the articulated legs about the two mutually 
perpendicular axes and motor control circuit (142) receiving the output of 
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the forward kinematics circuit and to produce the input control signal 
(144) to the first and second motive means for each articulated leg. 

9. Apparatus according to claim 8 further including a tachometer (53-54,63-64»83-84) 
5 for each first and second motive means operable to provide a feedback signal (140) to the 

motor control circuit. 

10, Apparatus according to claim 9 further including means (60) associated with the 
processor means to provide the information necessary for the processor to perform the 

10 characterization of the signals in accordance with the operator's desires. 
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